//Steve Tannahill 100697073

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>

#include <native/task.h>
#include <native/timer.h>
#include <rtdk.h>
#include <native/sem.h>
#include <time.h>
#include <native/mutex.h>
#include <native/cond.h>

RT_MUTEX status_M, dropQueue_M, pickupQueue_M, statusBuffer_M[2];
RT_COND empty, full;
RT_SEM stop;

RT_TASK supervisor_task;
RT_TASK floorRun_task;

int systemSpeed = 1; //speed up testing, 1=real time, >1 faster, <1 slower

int NumberOfElevators;
int elevatorNumber;
int NumberOfFloors; 
int currentFloor;
int nextFloor;
int direction; // up (1), down(-1), idle(0)
int elevatorState;//states: moving(0), stopped(1), door opening(2), door open(3), door closing(4)

int * dropQueue;  //includes set of floors for both directions
int * pickupQueue;  //includes one set of floors


char * statusBuffer[2];


void printPickUpQueue(){
	int i;
	for(i = 0; i<2*NumberOfFloors;i++){
		rt_printf("%d-%d\n", i, pickupQueue[i]);
	}
}
void setDropQueue(int index, int value){
	rt_mutex_acquire(&dropQueue_M,TM_INFINITE);
	dropQueue[index] = value;
	rt_mutex_release(&dropQueue_M);
	if(direction == 0){
		rt_sem_v(&stop);
	}
}
int readDropQueue(int index){
	int tmp;
	rt_mutex_acquire(&dropQueue_M,TM_INFINITE);
	tmp = dropQueue[index];
	rt_mutex_release(&dropQueue_M);
	return tmp;
}
void setpickupQueue(int index, int value){
	rt_printf("in set pickup queue");
	rt_mutex_acquire(&pickupQueue_M,TM_INFINITE);
	pickupQueue[index] = value;
	rt_mutex_release(&pickupQueue_M);
	if(direction == 0){
		rt_sem_v(&stop);
	}
}
int readpickupQueue(int index){
	int tmp;
	rt_mutex_acquire(&pickupQueue_M,TM_INFINITE);
	tmp = pickupQueue[index];
	rt_mutex_release(&pickupQueue_M);
	return tmp;
}
void updateVariable(int *var, int value, char *varName){
	rt_mutex_acquire(&status_M,TM_INFINITE);
	//rt_printf("Set %s = %d\n",varName, value);
	*var = value;
	rt_mutex_release(&status_M);
}
int readVariable(int *var){
	int temp;
	rt_mutex_acquire(&status_M,TM_INFINITE);
	temp = *var;
	rt_mutex_release(&status_M);
	return temp;
}

int dropRequest(int dir, int floor){
	if(currentFloor*dir < floor*dir){
		dropQueue[floor-1] = 1;
		return 1;
	}else{
		return 0;
	}	
}
void pickUpRequest(int dir, int floor){
	rt_printf("PickupRequest: %d, %d\n", dir, floor);
	if(dir == 1){
		pickupQueue[floor-1] = 1;
	}else{
		pickupQueue[2*NumberOfFloors-floor] = 1;
	}
}
void removeFromQueue(int dir, int floor){
	rt_printf("Remove: %d/%d\n", dir, floor);
	//remove from drop Queue
	dropQueue[floor-1] = 0;
	//remove from pick up Queue 
	if(dir == 1){
		
		pickupQueue[floor-1] = 0;
	}else{
		
		pickupQueue[2*NumberOfFloors-floor] = 0;
	}
}

int getNextFloor(dir){
	int i;
	if(dir == 1){	
		//next floor in current direction
		for(i=currentFloor; i<NumberOfFloors;i++){
			if((dropQueue[i-1] ==1) | (pickupQueue[i] == 1)){
				direction =1;
				return i+1;
			}
		}
		//next floor in other direction
		for(i=NumberOfFloors; i<2*NumberOfFloors; i++){
			if(pickupQueue[i] == 1){
				direction =-1;
				return 2*NumberOfFloors - i;
			}
		}
		for(i=0; i<currentFloor; i++){
			if(pickupQueue[i] == 1){
				direction = 1;
				return i+1;
			}
		}
	}else{
		//next floor in current direction
		for(i=currentFloor; i>-1;i--){
			if((dropQueue[i-1] ==1) | (pickupQueue[2*NumberOfFloors-i] == 1)){
				direction = -1;
				return i;
			}
		}
		//next floor in other direction
		for(i=0; i<NumberOfFloors; i++){
			if(pickupQueue[i] == 1){
				direction = 1;
				return i+1;
			}
		}
		for(i=NumberOfFloors; i<NumberOfFloors+currentFloor; i++){
			if(pickupQueue[i] == 1){
				direction = -1;
				return (2*NumberOfFloors - i);
			}
		}		
	}
	return currentFloor;
}



//communications thread 

//status Thread
void updateStatus(){
	int i;
	int j;
	while(1){
		if(i == 1){
			i =0;
		}else{
			i = 1;
		}
		rt_mutex_acquire(&statusBuffer_M[i],TM_INFINITE);
		//statusBuffer[i][0] = UPDATE;
		statusBuffer[i][1] = (char)readVariable(&elevatorNumber);
		statusBuffer[i][2] = (char)readVariable(&currentFloor);
		statusBuffer[i][3] = (char)readVariable(&nextFloor);
		for(j=3;j<2*NumberOfElevators+4;j++){
			statusBuffer[i][j] = pickupQueue[i-3];
		}
		statusBuffer[i][2*NumberOfElevators+4] = '\n';
		rt_mutex_release(&statusBuffer_M[i]);
		//sleep 75ms
		
	}
}

void elevatorStop(){
	
	//elevator stops
	updateVariable(&elevatorState, 1, "elevatorState");
	removeFromQueue(direction, currentFloor);
	//doors opening
	updateVariable(&elevatorState, 2, "elevatorState");
	rt_task_sleep(1000000/systemSpeed);
	//doors open
	updateVariable(&elevatorState, 3, "elevatorState");
	rt_task_sleep(1000000/systemSpeed);
	//doors closing
	updateVariable(&elevatorState, 4, "elevatorState");
	rt_task_sleep(1000000/systemSpeed);
	rt_printf("Direction: %d\n", direction);
}
void elevatorRun(int dir){
	updateVariable(&elevatorState, 0, "elevatorState");
	while(1){
		
		updateVariable(&currentFloor, currentFloor + dir, "Current Floor");
		rt_printf("C: %d, N: %d, D: %d\n", currentFloor, nextFloor, dir);
		if(currentFloor == nextFloor){
			rt_printf("STOPPING\n");
			elevatorStop();
			break;
		}
		updateVariable(&nextFloor, getNextFloor(dir), "Next Floor");
		rt_task_sleep(1000000000/systemSpeed);
	}		
}

//floorrun Thread
void floorRun(void *arg){
	while(1){
		updateVariable(&nextFloor, getNextFloor(direction), "Next Floor");
		if(currentFloor > nextFloor){
			rt_printf("Going Down\n");
			elevatorRun(-1);
		}else if(currentFloor < nextFloor) {
			rt_printf("Going Up\n");
			elevatorRun(1);
		}else{
			rt_printf("Stopped\n");
			direction = 0;
			//wait for signal
			rt_sem_p(&stop,TM_INFINITE);
			//sleep(2000);
			//break;
		}
	}	
}

//supervisor Thread
void supervisor(void *arg){
	//wait for a signal 
	//add requests to pickup queue
	int i;
	pickUpRequest(1,15);
	for(i = NumberOfFloors-1;i>0;i = i-NumberOfElevators){
		pickUpRequest(-1,i);
	}
	printPickUpQueue();
	//pickUpRequest(1,0);
}

void catch_signal(int sig)
{
}

void Testing(){
	
	int i;
	//rt_task_set_periodic(NULL, TM_NOW, 1000000000);
	//no requests - elevator not moving
	rt_printf("------------Test 1 -------------\n");
	rt_printf("----------Test1: Success---------------\n");
	rt_printf("");
	//Hall Call request - elevator moves to requested floor
	rt_printf("------------Test 2 -------------\n");
	rt_printf("Hall Call request- simulation\n");
	rt_printf("Start Elevator at Floor 1...\n");
	currentFloor = 1;
	rt_printf("Receive Hall Call to Floor 5\n");
	pickUpRequest(1, 5);
	//sleep
	//for(i = 0; i<10;i++){
		//rt_task_wait_period(NULL);
		rt_task_sleep(100000000);
	//}
		
	rt_printf("Elevator is on Floor #: %d\n", currentFloor);
	while(currentFloor != 5){
		rt_task_sleep(100000000);
	}
	rt_printf("----------Test2: Success---------------\n");
	rt_printf("------------Test 3 -------------\n");
	rt_printf("Change Direction\n");
	rt_printf("Receive Hall Call to Floor 10 going down\n");
	pickUpRequest(-1, 10);
	rt_printf("Receive Hall Call to Floor 5 going down\n");
	pickUpRequest(-1, 4);
	while(currentFloor != 4){
		rt_task_sleep(100000000);
	}
	rt_printf("----------Test3: Success---------------\n");
	//pickUpRequest(1, 10);
	//drop request - elevator moves to requested floor

	//elevator stops moving and no requests
	//change direction (request
	
}

int main(int argc, char* argv[])
{

	mlockall(MCL_CURRENT|MCL_FUTURE);
	signal(SIGTERM, catch_signal);
	signal(SIGINT, catch_signal);
	rt_print_auto_init(1);
	srand(time(NULL));
	/* -------------------
	Main Arguments
	int elevatorNumber
	int NumberOfElevators
	int NumberOfFloors
	----------------------*/
	//initialize variables 
	elevatorNumber = 2;
	NumberOfElevators = 4;
	NumberOfFloors = 30;
	
	

	pickupQueue = realloc(pickupQueue, NumberOfFloors*2*sizeof(int));
	dropQueue = realloc(dropQueue, NumberOfFloors*sizeof(int));
	statusBuffer[0] = realloc(statusBuffer[0], (NumberOfFloors*2+5)*sizeof(char));
	statusBuffer[1] = realloc(statusBuffer[1], (NumberOfFloors*2+5)*sizeof(char));
	currentFloor = 1;
	nextFloor = 1;
	direction = 1;
	//nextFloor = 5;
	//init("localhost", "5000");
	
	rt_sem_create(&stop,NULL,1,S_FIFO);
	//status_M, dropQueue_M, pickupQueue_M, statusBuffer_M[2]
	rt_mutex_create(&status_M,NULL);
	rt_mutex_create(&dropQueue_M,NULL);
	rt_mutex_create(&pickupQueue_M,NULL);
	rt_mutex_create(&statusBuffer_M[0],NULL);
	rt_mutex_create(&statusBuffer_M[1],NULL);
	//rt_task_create(&supervisor_task, NULL, 0, 99, T_JOINABLE);
	rt_task_create(&floorRun_task, NULL, 0, 99, T_JOINABLE);

	//rt_task_start(&supervisor_task, &supervisor, NULL);
	rt_task_start(&floorRun_task, &floorRun, NULL);

	//run floor run tests
	//sleep(1000);
	Testing();

	//rt_task_join(&supervisor_task);
	rt_task_join(&floorRun_task);

	

	pause();
	//rt_task_delete(&supervisor_task);
	rt_task_delete(&floorRun_task);
	rt_printf("DONE\n");
	return(0);

}
